#include "blobDetection.h"

BlobDetection::BlobDetection(ros::NodeHandle &n)
	: pixelTransform(n)
{

}

void BlobDetection::updateModel(const sensor_msgs::CameraInfoConstPtr &camInfo)
{
	pixelTransform.UpdateModel(camInfo);
}

/**************************************************************
  Function for detecting blobs within the input image. The coordinates of
  the detected blobs will be converted from pixel coordinates to real coordinates
  with the pixelTransform class.
**************************************************************/
void BlobDetection::detectBlobs(cv::Mat &image)
{
	mObjects.clear();

	vector<Blob> detectedBlobs;

	filterImage(image, detectedBlobs);
	if(detectedBlobs.size() > 0)
	{
		Blob areaBlob;

		// Set default values
		areaBlob.area = 0;
		areaBlob.center = cv::Point2d(0,0);


		// Look for the blob with the largest area
		for(size_t t = 0; t < detectedBlobs.size(); t++)
		{

			if(detectedBlobs[t].area > areaBlob.area)
				areaBlob = detectedBlobs[t];
		}

		// Filter the postion of the blob with a simple Median filter
		//areaBlob = filterBlobPosition(areaBlob);

		// Create the object using the blob information
		createObject(areaBlob.center);
	}
}


void BlobDetection::createObject(cv::Point2d blob)
{
	cv::Point3d blobPos;

	pixelTransform.transformFromPixelTo3d(blob, blobPos);

	mObjects.push_back(detectedObject(blobPos, blob));
}


void BlobDetection::setHuecolorSpan(int hue, int span, int saturation)
{
	mHueColor = hue;
	mHueSpan = span;
	mSaturation = saturation;
}


/**************************************************************
  Function for filtering the positon of the found blob using the
  previous known position(s) of the blob.
**************************************************************/
Blob BlobDetection::filterBlobPosition(Blob newBlob)
{
	mPrevBlobs.push_back(newBlob);

	if(mPrevBlobs.size() > 10)
		mPrevBlobs.pop_front();

	cv::Point2d sumPoints;
	for(size_t t = 0; t < mPrevBlobs.size(); t++)
		sumPoints += mPrevBlobs[t].center;

	sumPoints.x = sumPoints.x / mPrevBlobs.size();
	sumPoints.y = sumPoints.y / mPrevBlobs.size();
	newBlob.center = sumPoints;

	return newBlob;
}


/**************************************************************
  Function for filter out the correct color blob within the image
**************************************************************/
void BlobDetection::filterImage(cv::Mat &image, vector<Blob> &detectedBlobs)
{
    cv::Mat imgHsv;
	cv::Mat mFilteredImage;
	IplImage mImageFilterIp;

    int hueLowBound;
    int hueHighBound;

    cv::cvtColor(image, imgHsv, CV_BGR2HSV);
    cv::cvtColor(image, mFilteredImage, CV_BGR2GRAY);

    // Set the hue-color and saturation values for the filtering process
    hueLowBound = mHueColor - mHueSpan/2;
    hueHighBound = mHueColor + mHueSpan/2;

    // Zero Matrices
    cv::Mat imgHue = cv::Mat::zeros(imgHsv.rows, imgHsv.cols, CV_8U);
    cv::Mat imgSat = cv::Mat::zeros(imgHsv.rows, imgHsv.cols, CV_8U);
    cv::Mat imgVal = cv::Mat::zeros(imgHsv.rows, imgHsv.cols, CV_8U);

    // HSV Channel 0 -> img_hue_ & HSV Channel 1 -> img_sat_
    int from_to[] = { 0,0, 1,1, 2,2};
    cv::Mat img_split[] = { imgHue, imgSat, imgVal};
    cv::mixChannels(&imgHsv, 1,img_split,3,from_to,3);

    for(int i = 0; i < imgHsv.rows; i++)
    {
		for(int j = 0; j < imgHsv.cols; j++)
		{
			// Take out the colors cyan and magenta from the picture
			uchar hue = imgHue.at<uchar>(i,j);
			uchar sat = imgSat.at<uchar>(i,j);

			if((hue > hueLowBound &&
				hue < hueHighBound &&
				sat > mSaturation))
				mFilteredImage.at<uchar>(i,j) = 0;
			else
				mFilteredImage.at<uchar>(i,j) = 255;
		}
    }


    /*********************************************************************
    * Do morphological operations on the filtered image
    * to smooth out the black pixels that still can be in the image after
    * taking out other colors except the one that we are looking for in
    * the previous stage.
    *********************************************************************/


    /* Morphological operations on the image that makes the black pixels more diluted. So the ones that are alone
    will disappear*/
    cv::Size strel_size;
    strel_size.width = 3;
    strel_size.height = 3;
    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE,strel_size);

    cv::morphologyEx(mFilteredImage, mFilteredImage,cv::MORPH_CLOSE,strel,cv::Point(-1, -1),2);
    cv::morphologyEx(mFilteredImage, mFilteredImage,cv::MORPH_OPEN,strel,cv::Point(-1, -1),3);



    /**************************************************************************
    * Use the blob detection library to find the blobs in the image. It
    * looks after black pixels that are close together and assign them as a
    * blob.
    *
    * After finding the blobs the center of the detected blobs are written to
    * the original image using the circle function in openCV.
    **************************************************************************/
    mImageFilterIp = mFilteredImage;

    // Start the searching for blobs in the picture
    // Filter out the results. The area of the blob must be atleast 100 pixels
    CBlobResult blobResults = CBlobResult(&mImageFilterIp, NULL, 255);
    blobResults.Filter(blobResults, B_EXCLUDE, CBlobGetArea(), B_LESS, 100, 0);

    // Print the centers of the blobs and also fill the blob with black color
    for(int i = 0; i < blobResults.GetNumBlobs(); i++)
    {
		CBlob *wantedBlob = blobResults.GetBlob(i);
		wantedBlob->FillBlob(&mImageFilterIp, cv::Scalar(0,0,0));

		Blob b;
		CBlobGetXCenter centerX;
		CBlobGetYCenter centerY;

		b.center.x = centerX(*wantedBlob);
		b.center.y = centerY(*wantedBlob);
		b.area = wantedBlob->Area();

		// Save the detected blob in the vector that will be outputed from the function
		detectedBlobs.push_back(b);
    }

}
